/// 投影图像， 按照角度保存到 test 目录中
/// 全部使用VTK，并用imageViewer2显示
/// ./Vtk_ProjectionImage2 /works/src_works/z_make/testdata/head_s5 1 1000
/// 1 为读取实际dicom文件夹
/// 1000 为threshold
///
#include <vector>

#include <vtkObjectFactory.h>
#include <vtkSmartPointer.h>
#include <vtkTransform.h>
#include <vtkMatrix4x4.h>
#include <vtkMetaImageReader.h>
#include <vtkImageData.h>
#include <vtkImageCast.h>
#include <vtkDICOMImageReader.h>
#include <vtkPNGWriter.h>
#include <vtkImageViewer2.h>
#include <vtkInteractorStyleImage.h>
#include <vtkInteractorObserver.h>
#include <vtkActor2D.h>
#include <vtkTextProperty.h>
#include <vtkTextMapper.h>
#include <vtkRenderer.h>
#include <vtkRenderWindowInteractor.h>

#include <Eigen/Core>

using namespace std;

Eigen::Vector3d multiplyPoint(vtkMatrix4x4 *m, Eigen::Vector3d &p)
{
    Eigen::Vector3d outP;
    double *elem = m->GetData();

    double &v1 = p[0];
    double &v2 = p[1];
    double &v3 = p[2];

    outP[0] = v1 * elem[0] + v2 * elem[1] + v3 * elem[2];
    outP[1] = v1 * elem[4] + v2 * elem[5] + v3 * elem[6] ;
    outP[2] = v1 * elem[8] + v2 * elem[9] + v3 * elem[10];

    return outP;
}

Eigen::Vector3d multiplyPoint(vtkMatrix4x4 *m, double p[3])
{
    Eigen::Vector3d outP;
    double *elem = m->GetData();

    double &v1 = p[0];
    double &v2 = p[1];
    double &v3 = p[2];

    outP[0] = v1 * elem[0] + v2 * elem[1] + v3 * elem[2] + elem[3];
    outP[1] = v1 * elem[4] + v2 * elem[5] + v3 * elem[6] + elem[7];
    outP[2] = v1 * elem[8] + v2 * elem[9] + v3 * elem[10]+ elem[11];

    return outP;
}

// 计算位置，并偏移到合适的位置
Eigen::Vector2d projectionPoint(vtkMatrix4x4 *m, double p[3], double u0, double v0)
{
    Eigen::Vector2d outP;
    double *elem = m->GetData();

    double &v1 = p[0];
    double &v2 = p[1];
    double &v3 = p[2];

    double Zc = v1 * elem[8] + v2 * elem[9] + v3 * elem[10]+ elem[11];

    outP[0] = (v1 * elem[0] + v2 * elem[1] + v3 * elem[2] + elem[3])/Zc + u0;
    outP[1] = (v1 * elem[4] + v2 * elem[5] + v3 * elem[6] + elem[7])/Zc + v0;


    return outP;
}

// 计算位置，并偏移到合适的位置
void projectionPoint(vtkMatrix4x4 *m, double p[3], double u0, double v0, double outP[2])
{
    double *elem = m->GetData();

    double &v1 = p[0];
    double &v2 = p[1];
    double &v3 = p[2];

    double Zc = v1 * elem[8] + v2 * elem[9] + v3 * elem[10]+ elem[11];

    outP[0] = (v1 * elem[0] + v2 * elem[1] + v3 * elem[2] + elem[3])/Zc + u0;
    outP[1] = (v1 * elem[4] + v2 * elem[5] + v3 * elem[6] + elem[7])/Zc + v0;
}

/// x,y,z 传入的是弧度脚, 偏移量
vtkSmartPointer<vtkMatrix4x4> createMatrixEulerZYX(Eigen::Vector3d &anglesInRadian, Eigen::Vector3d &center, Eigen::Vector3d &translation)
{
    vtkSmartPointer<vtkMatrix4x4> m = vtkMatrix4x4::New();

    double u = anglesInRadian[2];
    double v = anglesInRadian[1];
    double w = anglesInRadian[0];
    /**
     u, v, w 绕 z, y, x
    [
        cv * cu     sw*sv*cu - cw*su   cw*sv*cu + sw*su
        cv * su     sw*sv*su + cw*cu   cw*sv*su - sw*cu
        -sv         sw * cv            cw * cv
    ]
     */

    double cu = cos(u);
    double su = sin(u);
    double cv = cos(v);
    double sv = sin(v);
    double cw = cos(w);
    double sw = sin(w);

    m->Element[0][0] = cv * cu;
    m->Element[1][0] = cv * su;
    m->Element[2][0] = -sv;
    m->Element[3][0] = 0;

    m->Element[0][1] = sw*sv*cu - cw*su;
    m->Element[1][1] = sw*sv*su + cw*cu;
    m->Element[2][1] = sw * cv;
    m->Element[3][1] = 0;

    m->Element[0][2] = cw*sv*cu + sw*su;
    m->Element[1][2] = cw*sv*su - sw*cu;
    m->Element[2][2] = cw * cv;
    m->Element[3][2] = 0;

    m->Element[0][3] = 0;
    m->Element[1][3] = 0;
    m->Element[2][3] = 0;
    m->Element[3][3] = 1;

    Eigen::Vector3d offset = multiplyPoint(m, center);

    m->Element[0][3] = translation[0]-offset[0];
    m->Element[1][3] = translation[1]-offset[1];
    m->Element[2][3] = translation[2]-offset[2];
    m->Modified();

    return m;
}

vtkSmartPointer<vtkMatrix4x4> createHMatrix(Eigen::Vector3d &center, Eigen::Vector3d &translation,
                                            double f, double dx, double dy,
                                            Eigen::Vector3d &anglesInDegree)
{

    Eigen::Vector3d anglesInRadian = anglesInDegree * M_PI/ 180.0;
    vtkSmartPointer<vtkMatrix4x4> m = createMatrixEulerZYX(anglesInRadian, center, translation);

    // 先按照1:1 像素生成
    //vtkSmartPointer<vtkMatrix4x4> mUVfrc = vtkMatrix4x4::New();
    double fx = f/dx, fy = f/dy;
    m->Element[0][0] *=fx ;
    m->Element[0][1] *=fx ;
    m->Element[0][2] *=fx ;
    m->Element[0][3] *=fx ;

    m->Element[1][0] *=fy ;
    m->Element[1][1] *=fy ;
    m->Element[1][2] *=fy ;
    m->Element[1][3] *=fy ;


    return m;
}


static void demoProjectPoints()
{

    std::vector<Eigen::Vector3d> pts = {
        {0, 0, 0}, {0, 20, 0},
        {20, 0, 0}, {20, 20, 0},
        {0, 0, 10}, {0, 20, 10},
        {20, 0, 10}, {20, 20, 10},
    };

    Eigen::Vector3d translation = {0, 0, 100};
    Eigen::Vector3d center = {10, 10, 0};
    Eigen::Vector3d angleInDegree = {-90, 0, 0};

    double f = 1;
    double dx = 1, dy = 1; // 1mm 对应1个像素
    double u0 = 20, v0 = 20;
    vtkSmartPointer<vtkMatrix4x4> m = createHMatrix(center, translation, f, dx, dy, angleInDegree);
    m->Print(std::cout);
    for (auto it = pts.begin();it != pts.end(); it++) {
        Eigen::Vector3d proP = multiplyPoint(m, it->data());

        std::cout << "projection: "
                  << proP[0] << " "
                  << proP[1] << " "
                  << proP[2] << " "
                  << std::endl;
    }
}

template<typename _Tp>
vtkSmartPointer<vtkImageData> projectImage(vtkImageData *volume, vtkMatrix4x4 *m, std::string &outputFilename, int w, int h, int threshold=1)
{
    int dims[3];
    volume->GetDimensions(dims);
    double range[2];
    volume->GetScalarRange(range);
    double maxvalue = range[1] - threshold;
    double spacing[3];
    volume->GetSpacing(spacing);

    vtkSmartPointer<vtkImageData> image =  vtkSmartPointer<vtkImageData>::New();
    image->SetDimensions(w,h,1);
    image->AllocateScalars(VTK_DOUBLE,1);

    for (int y = 0; y < h; y++)
    {
        for (int x = 0; x < w; x++)
        {
            double* pixel = static_cast<double*>(image->GetScalarPointer(x,y,0));
            pixel[0] = 0;
        }
    }

    _Tp* pData = static_cast<_Tp*>(volume->GetScalarPointer());
    for (int z = 0; z < dims[2]; z++)
    {
        for (int y = 0; y < dims[1]; y++)
        {
            for (int x = 0; x < dims[0]; x++)
            {
                _Tp pixel = pData[x + y*dims[0] + z*dims[0]*dims[1]];
                if (pixel < threshold) {
                    continue;
                }
                //unsigned char newpixel = 255*(pixel - threshold)/maxvalue;
                double newpixel = pixel;
                double p[3] = {double(x*spacing[0]), double(y*spacing[1]), double(z*spacing[2])};
                double out[2];
                projectionPoint(m, p, w/2, h/2, out);
                // 取图像中最大值作为投影
                int ox = out[0], oy = out[1];
                if (ox >= 0 && ox<w && oy >= 0 && oy < h) {
                    double *pv2 = static_cast<double*>(image->GetScalarPointer(ox, oy, 0));
                    //std::cout << "xxx:" << pv2 << endl;
                    pv2[0] = pv2[0] + newpixel;
                }
            }
            //std::cout << std::endl;
        }
        //std::cout << std::endl;
    }

    image->Modified();
    double range2[2];
    image->GetScalarRange(range2);
    cout << "image range: " << range2[0] << ", " << range2[1]  << endl;

    for (int y = 0; y < h; y++)
    {
        for (int x = 0; x < w; x++)
        {
            double* pixel = static_cast<double*>(image->GetScalarPointer(x,y,0));
            pixel[0] = pixel[0]*255.0/range2[1];
        }
    }
    image->Modified();


    vtkSmartPointer<vtkImageCast> castFilter = vtkSmartPointer<vtkImageCast>::New();
    castFilter->SetInputData(image);
    castFilter->SetOutputScalarTypeToUnsignedChar();
    castFilter->Update();

#if 1  // save to png
    // 保存图像
    vtkSmartPointer<vtkPNGWriter> writer = vtkSmartPointer<vtkPNGWriter>::New();
    writer->SetFileName( outputFilename.c_str() );
    writer->SetInputConnection(castFilter->GetOutputPort());

    try
    {
        writer->Write();
        std::cout << "Write to " << outputFilename << std::endl;
    }
    catch (exception & error)
    {
        std::cerr << "Error: " << error.what() << std::endl;
        //return EXIT_FAILURE;
    }
    //return EXIT_SUCCESS;
#endif
    return image;
}

// helper class to format slice status message
class StatusMessage {
public:
    static std::string format(int angleX, int angleY) {
        std::stringstream tmp;
        tmp << "Rotate(X/Y): " << angleX << "/" << angleY;
        return tmp.str();
    }
};

// Define own interaction style
class SliceVtkInteractorStyleImage : public vtkInteractorStyleImage
{
public:
    static SliceVtkInteractorStyleImage* New();
    vtkTypeMacro(SliceVtkInteractorStyleImage, vtkInteractorStyleImage);

public:
    vtkImageData * volume;
    vtkImageViewer2* imageViewer;
    vtkTextMapper* statusMapper;
    int step = 1;
    int angleX = 0;
    int angleY = 0;
    int angleZ = 0;
    int w = 400, h = 400;
    double threshold = 500;
    Eigen::Vector3d translation = {0, 0, 1000};
    double f = 1600;
    double dx = 1, dy = 1; // 1mm 对应1个像素

public:
    void setVolume(vtkImageData *_volume) {
        volume = _volume;
    }
    void setImageViewer(vtkImageViewer2* _imageViewer) {
        imageViewer = _imageViewer;
    }

    void setStatusMapper(vtkTextMapper* _statusMapper) {
        statusMapper = _statusMapper;
    }

    void swithImage() {
        int dims[3];
        volume->GetDimensions(dims);
        double spacing[3];
        volume->GetSpacing(spacing);
        Eigen::Vector3d center = {dims[0]*spacing[0]/2, dims[1]*spacing[1]/2, dims[2]*spacing[2]/2};
        Eigen::Vector3d translation = {0, 0, 1000};
        double f = 1600;
        double dx = 1, dy = 1; // 1mm 对应1个像素

        Eigen::Vector3d angleInDegree = {angleX, angleY, angleZ};
        vtkSmartPointer<vtkMatrix4x4> m = createHMatrix(center, translation, f, dx, dy, angleInDegree);
        char buf[200];
        snprintf(buf, 200, "test/proj_%03d_%03d.png", angleY, angleX);
        std::string outputFilename = buf;

        vtkSmartPointer<vtkImageData> img;
        switch (volume->GetScalarType()) {
        case VTK_SHORT:
            img = projectImage<short>(volume, m, outputFilename, w, h, threshold);
            break;
        default:
            img = projectImage<unsigned char>(volume, m, outputFilename, w, h);
            break;
        }
        std::string msg = StatusMessage::format(angleX, angleY);
        statusMapper->SetInput(msg.c_str());
        imageViewer->SetInputData(img);
        imageViewer->Render();
    }


protected:
    void moveLeft() {
        angleX -= step;
        swithImage();
    }
    void moveRight() {
        angleX += step;
        swithImage();
    }
    void moveUp() {
        angleY -= step;
        swithImage();
    }
    void moveDown() {
        angleY += step;
        swithImage();
    }

    void rotateZP() {
        angleZ += step;
        swithImage();
    }

    void rotateZN() {
        angleZ -= step;
        swithImage();
    }

    virtual void OnKeyDown() override {
        std::string key = this->GetInteractor()->GetKeySym();
        if(key.compare("Left") == 0) {
            moveLeft();
        } else if(key.compare("Right") == 0) {
            moveRight();
        } else if(key.compare("Up") == 0) {
            moveUp();
        } else if(key.compare("Down") == 0) {
            moveDown();
        } else if(key.compare("o") == 0) {
            rotateZP();
        } else if (key.compare("p") == 0) {
            rotateZN();
        } else if (key.compare("r") == 0) {
            angleZ = 0;
            swithImage();
        }
        // forward event
        vtkInteractorStyleImage::OnKeyDown();
    }

    virtual void OnMouseMove() override {
        // forward event
        vtkInteractorStyleImage::OnMouseMove();
    }
};

vtkStandardNewMacro(SliceVtkInteractorStyleImage);

int main(int argc, char *argv[])
{

    //demoProjectPoints();
    //return 0;

    std::string inputFilename;
    double threshold = 5;
    int option = 0;
    if(argc < 2)
    {
      std::cerr << "Required arguments: image.mha" << std::endl;
      return EXIT_FAILURE;
    }
    inputFilename = argv[1];
    if (argc > 2) {
        option = atoi(argv[2]);
    }

    if (argc > 3) {
        threshold = atof(argv[3]);
    }

    vtkSmartPointer<vtkMetaImageReader> readerMHa = vtkSmartPointer<vtkMetaImageReader>::New();
    vtkSmartPointer<vtkDICOMImageReader> readerDcm = vtkSmartPointer<vtkDICOMImageReader>::New();
    vtkImageData *volume = nullptr;
    if (option == 0) {
        readerMHa->SetFileName(inputFilename.c_str());
        readerMHa->Update();

        volume = readerMHa->GetOutput();
    } else if (option == 1) {
        readerDcm->SetDirectoryName(inputFilename.c_str());
        readerDcm->Update();
        volume = readerDcm->GetOutput();
    } else {
        cout << "Not supported type." << std::endl;
        return EXIT_FAILURE;
    }

    double origin[3];
    volume->GetOrigin(origin);
    cout << "origin: " << origin[0] << ", " << origin[1] << ", " << origin[2] << endl;
    double spacing[3];
    volume->GetSpacing(spacing);
    cout << "spacing: " << spacing[0] << ", " << spacing[1] << ", " << spacing[2] << endl;
    int dims[3];
    volume->GetDimensions(dims);
    cout << "dimensions: " << dims[0] << ", " << dims[1] << ", " << dims[2] << endl;
    int extent[6];
    volume->GetExtent(extent);
    cout << "extent: " << extent[0] << ", " << extent[1] << ", " << extent[2] << ", " <<
            extent[3] << ", " << extent[4] << ", " << extent[5] << endl;

    // 获取原图像数据的灰度值范围
    // 如：CT值的单位是Hounsfield，简称为Hu，范围是-1024-3071。用于衡量人体组织对X射线的吸收率，设定水的吸收率为0Hu。
    double range[2];
    volume->GetScalarRange(range);
    cout << "range: " << range[0] << ", " << range[1] << endl;
    cout << "GetScalarTypeAsString (): " << volume->GetScalarTypeAsString () << endl;          

    //Eigen::Vector3d focus = {dims[0]/2, dims[1]/2, 100};
    Eigen::Vector3d center = {dims[0]*spacing[0]/2, dims[1]*spacing[1]/2, dims[2]*spacing[2]/2};
    Eigen::Vector3d translation = {0, 0, 1000};
    double f = 1600;
    double dx = 1, dy = 1; // 1mm 对应1个像素

    // 投影图像
    int w = 400, h = 400;
    int angleX = 0;
    int angleY = 0;

    // first ->
    Eigen::Vector3d angleInDegree = {angleX, angleY, 0};

    vtkSmartPointer<vtkMatrix4x4> m = createHMatrix(center, translation, f, dx, dy, angleInDegree);
    m->Print(std::cout);
    char buf[200];
    snprintf(buf, 200, "test/proj_%03d_%03d.png", angleY, angleX);
    std::string outputFilename = buf;

    vtkSmartPointer<vtkImageData> image;
    switch (volume->GetScalarType()) {
    case VTK_SHORT:
        image = projectImage<short>(volume, m, outputFilename, w, h, threshold);
        break;
    default:
        image = projectImage<unsigned char>(volume, m, outputFilename, w, h);
        break;
    }

    // Visualize
    vtkSmartPointer<vtkImageViewer2> imageViewer = vtkSmartPointer<vtkImageViewer2>::New();
    imageViewer->SetInputData(image);

    double window = imageViewer->GetColorWindow();
    double level = imageViewer->GetColorLevel();
    cout << "window: " << window << " level: " << level << endl;

    // slice status message
    vtkSmartPointer<vtkTextProperty> sliceTextProp = vtkSmartPointer<vtkTextProperty>::New();
    sliceTextProp->SetFontFamilyToCourier();
    sliceTextProp->SetFontSize(20);
    sliceTextProp->SetVerticalJustificationToBottom();
    sliceTextProp->SetJustificationToLeft();

    vtkSmartPointer<vtkTextMapper> sliceTextMapper = vtkSmartPointer<vtkTextMapper>::New();
    std::string msg = StatusMessage::format(angleX, angleY);
    sliceTextMapper->SetInput(msg.c_str());
    sliceTextMapper->SetTextProperty(sliceTextProp);

    vtkSmartPointer<vtkActor2D> sliceTextActor = vtkSmartPointer<vtkActor2D>::New();
    sliceTextActor->SetMapper(sliceTextMapper);
    sliceTextActor->SetPosition(15, 10);

    // create an interactor with our own style (inherit from vtkInteractorStyleImage)
    // in order to catch mousewheel and key events
    vtkSmartPointer<vtkRenderWindowInteractor> renderWindowInteractor =
            vtkSmartPointer<vtkRenderWindowInteractor>::New();

    vtkSmartPointer<SliceVtkInteractorStyleImage> sliceInteractorStyle =
            vtkSmartPointer<SliceVtkInteractorStyleImage>::New();

    sliceInteractorStyle->setVolume(volume);
    sliceInteractorStyle->setImageViewer(imageViewer);
    sliceInteractorStyle->setStatusMapper(sliceTextMapper);
    sliceInteractorStyle->f = f;
    sliceInteractorStyle->step = 1;
    sliceInteractorStyle->w = w;
    sliceInteractorStyle->h = h;
    sliceInteractorStyle->threshold = threshold;
    sliceInteractorStyle->translation = translation;
    sliceInteractorStyle->angleX = 0;
    sliceInteractorStyle->angleY = 0;

    imageViewer->SetupInteractor(renderWindowInteractor);

    renderWindowInteractor->SetInteractorStyle(sliceInteractorStyle);
    imageViewer->GetRenderer()->AddActor2D(sliceTextActor);

    // initialize rendering and interaction
    //imageViewer->GetRenderWindow()->SetSize(400, 300);
    //imageViewer->GetRenderer()->SetBackground(0.2, 0.3, 0.4);
    imageViewer->Render();
    imageViewer->GetRenderer()->ResetCamera();
    imageViewer->Render();
    renderWindowInteractor->Start();

    return 0;
}
